package tests;

import java.awt.AWTException;

import SimpleOpenNI.SimpleOpenNI;
import SimpleOpenNI.SimpleOpenNIConstants;
import particleField.*;
import processing.core.PApplet;
import processing.core.PImage;
import processing.core.PVector;

public class Kin_following_paricles extends PApplet{
	ParticleSystem ps;
	int width = 640;
	int height = 480;
	PImage newImg; 
	public SimpleOpenNI  context;
	int offset = 0;
	boolean mod;
	
	public void setup()
	{
	 // context = new SimpleOpenNI(this);
		
	  context = new SimpleOpenNI(this,SimpleOpenNI.RUN_MODE_MULTI_THREADED);
	  
	  // enable depthMap generation 
	  context.enableDepth();
	  context.enableRGB();
	  
	  // enable skeleton generation for all joints
	  context.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);
	 
	  background(200,0,0);

	  stroke(0,0,255);
	  strokeWeight(3);
	  smooth();
	  
//	  size(context.depthWidth(), context.depthHeight());
	  size(width, height);
	  
	  ps = new ParticleSystem(1, new PVector(width,height), this);
	  
//	  int distance = 300;
//	  for (int i = distance/2; i < height; i+=distance) {
//		  for (int j = distance/2; j < width; j+=distance) {
//			  ps.add_magnet(this, new PVector(j, i), distance, false);
//		  }		
//	  }
	  
	}
	
	public void draw() {
		context.update();
		context.setMirror(true);
		background(255);
		
		if(frameCount % 100 == 0){
			if(mod){
				ps.force_home();
				mod = !mod;
			}else{
				ps.force_disable_home();
				mod = !mod;
			}
			
		}
		


		

//		image(context.rgbImage(),0,0);
		
//		PVector v1 = new PVector(100, 100 + (offset++)%300); //y, x
//		PVector v2 = new PVector(300, 200); //y, x
		
//		background(0x00FFFFFF);
//		ps.run(this);
//		for (int i = 0; i < 1; i++) {	
//			ps.addParticle(mouseX,mouseY);	
//		}

//		PVector pv1 = new PVector(200, 100);
//		PVector pv2 = new PVector(300, 300);		
//		generateAreaPoints(v1, v2, false);
		

		
		try {
			  
			  if(context.isTrackingSkeleton(1)){				
			    drawSkeleton(1);
//			  	drawCycle(1);
			  }
			  if(context.isTrackingSkeleton(2)){
				drawSkeleton(2);
//			  	drawCycle(2);
			  }
			  if(context.isTrackingSkeleton(3)){
				drawSkeleton(3);
			  	drawCycle(3);
			  }
		
				
			} catch (AWTException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			
			  
			
			ps.run(this);
			ps.add_magneticBox(this, 20);
			
	}
	
	public void drawCycle(int pers_no) throws AWTException{
		PVector hand_real = new PVector();
		PVector hand_pers = new PVector();
		
		context.getJointPositionSkeleton(pers_no, SimpleOpenNI.SKEL_LEFT_HAND, hand_real);
		context.convertRealWorldToProjective(hand_real, hand_pers);
		fill( color(0,0,0) );
		ellipse(hand_pers.x, hand_pers.y, 10, 10);

			
	}
	
	// draw the skeleton with the selected joints
	public void drawSkeleton(int userId)
	{
	  //rectMode (CENTER);
		stroke( color(0,0,0) );
		fill( color(0,0,0) );
	  
	  //mittelpunkte
	  PVector belly_real = new PVector();
	  PVector belly_pers = new PVector();
	  PVector head_real = new PVector();
	  PVector head_pers = new PVector();
	  
	  //alles links
	  PVector hand_l_real = new PVector();
	  PVector hand_l_pers = new PVector();
	  PVector elbow_l_real = new PVector();
	  PVector elbow_l_pers = new PVector();
	  PVector shoulder_l_real = new PVector();
	  PVector shoulder_l_pers = new PVector();
	  
	  PVector knee_l_real = new PVector();
	  PVector knee_l_pers = new PVector();
	  PVector foot_l_real = new PVector();
	  PVector foot_l_pers = new PVector();
	  
	  //alles rechts
	  PVector hand_r_real = new PVector();
	  PVector hand_r_pers = new PVector();
	  PVector elbow_r_real = new PVector();
	  PVector elbow_r_pers = new PVector();
	  PVector shoulder_r_real = new PVector();
	  PVector shoulder_r_pers = new PVector();
	  
	  PVector knee_r_real = new PVector();
	  PVector knee_r_pers = new PVector();
	  PVector foot_r_real = new PVector();
	  PVector foot_r_pers = new PVector();
	  
	  PVector real1 = new PVector();
	  PVector real2 = new PVector();
	  
	  //Koordinaten zuodnen
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND, real1);
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, real2);	  
	  generateAreaPoints(real1, real2, true);
	  
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, real1);
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, real2);	  
	  generateAreaPoints(real1, real2, true);
	  
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, real1);
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, real2);	  
	  generateAreaPoints(real1, real2, true);
	  
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, real1);
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, real2);	  
	  generateAreaPoints(real1, real2, true);
	  
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, real1);
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND, real2);	  
	  generateAreaPoints(real1, real2, true);
	  
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_NECK, real1);
	  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD, real2);	  
	  generateAreaPoints(real1, real2, true);
	  




//	  
//	  //winkel errechnen
//	  double ang_lHand_lElbow = Math.atan2( (hand_l_pers.y-elbow_l_pers.y), (hand_l_pers.x-elbow_l_pers.x) )*180/Math.PI;
//	  double x_middle = Math.abs(hand_l_pers.y-elbow_l_pers.y)/2 + hand_l_pers.y;
//	  double y_middle = Math.abs(hand_l_pers.x-elbow_l_pers.x)/2 + hand_l_pers.x;
//	  System.out.println( ang_lHand_lElbow );
//	  ellipseMode(CENTER);
//	  fill( color(255,255,255) );
//	  ellipse( (float)y_middle, (float)x_middle,  10, 10);

	  
	  
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);
//
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);
//
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND);
//
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
//
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);
//
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
//	  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);  
	}
	

	
	public void generateAreaPoints(PVector v1, PVector v2, boolean areRealValues){
		ellipseMode(CENTER);
		fill( color(0,0,0) );
		
		PVector p1 = v1;
		PVector p2 = v2;
		
		if(areRealValues){
			context.convertRealWorldToProjective(v1, p1);
			context.convertRealWorldToProjective(v2, p2);
		}
//		ellipse(p1.x, p1.y, 20, 20);
//		ellipse(p2.x, p2.y, 20, 20);
		  
		//winkel errechnen
		double angle = Math.atan2( (p2.y-p1.y), (p2.x-p1.x) )*180/Math.PI;

		
		int interate = 2;
		for (int i = 0; i <= interate; i++) {
			double x = 0;
			double y = 0;
			
			if(p2.y-p1.y > 0 && p2.x-p1.x > 0){
				//System.out.println("lo nach ru");
				y = Math.abs(p2.y-p1.y)/interate*i + getMin(p1.y, p2.y);
				x = Math.abs(p2.x-p1.x)/interate*i + getMin( p1.x, p2.x);
			}else if(p2.y-p1.y > 0 && p2.x-p1.x <= 0){
				//System.out.println("lu nach ro");
				y = Math.abs(p2.y-p1.y)/interate*i + getMin(p1.y, p2.y);
				x = -Math.abs(p2.x-p1.x)/interate*i + getMax( p1.x, p2.x);
			}else if(p2.y-p1.y <= 0 && p2.x-p1.x <= 0){
				y = Math.abs(p2.y-p1.y)/interate*i + getMin(p1.y, p2.y);
				x = Math.abs(p2.x-p1.x)/interate*i + getMin( p1.x, p2.x);
			}else if(p2.y-p1.y <= 0 && p2.x-p1.x > 0){
				y = Math.abs(p2.y-p1.y)/interate*i + getMin(p1.y, p2.y);
				x = -Math.abs(p2.x-p1.x)/interate*i + getMax( p1.x, p2.x);
			}
			fill( color(255,255,255) );
//			ellipse( (float)x, (float)y,  10, 10);
			
			fill( color(255,0,255) );
			
			int x_offs = getx_Polar(20, angle+90);
			int y_offs = gety_Polar(20, angle+90);
			
			if(frameCount%25 == 0){
				ps.add_magnet(this, new PVector((float)(x+x_offs), (float)(y+y_offs)), 40, false);
			}
			ellipse( (float)(x+x_offs), (float)(y+y_offs),  5, 5);
			ellipse( (float)(x-x_offs), (float)(y-y_offs),  5, 5);
			
		}

		
	}
	
	private int getx_Polar(double radius, double angle){
		return (int) (radius*(Math.cos(angle/360*2*Math.PI)) );
	}
	
	private int gety_Polar(double radius, double angle){
		return (int) (radius*(Math.sin(angle/360*2*Math.PI)) );
	}
	
	private float getMin(float a, float b){
		if(a > b){
			return b;
		}else{
			return a;
		}
	}
	
	private float getMax(float a, float b){
		if(a > b){
			return a;
		}else{
			return b;
		}
	}
	
	// -----------------------------------------------------------------
	// SimpleOpenNI events

	public void onNewUser(int userId)
	{
	  println("onNewUser - userId: " + userId);
	  println("  start pose detection");
	  
	  context.startPoseDetection("Psi",userId);
	}

	public void onLostUser(int userId)
	{
	  println("onLostUser - userId: " + userId);
	}

	public void onStartCalibration(int userId)
	{
	  println("onStartCalibration - userId: " + userId);
	}

	public void onEndCalibration(int userId, boolean successfull)
	{
	  println("onEndCalibration - userId: " + userId + ", successfull: " + successfull);
	  
	  if (successfull) 
	  { 
	    println("  User calibrated !!!");
	    context.startTrackingSkeleton(userId); 
	  } 
	  else 
	  { 
	    println("  Failed to calibrate user !!!");
	    println("  Start pose detection");
	    context.startPoseDetection("Psi",userId);
	  }
	}

	public void onStartPose(String pose,int userId)
	{
	  println("onStartPose - userId: " + userId + ", pose: " + pose);
	  println(" stop pose detection");
	  
	  context.stopPoseDetection(userId); 
	  context.requestCalibrationSkeleton(userId, true);
	 
	}

	public void onEndPose(String pose,int userId)
	{
	  println("onEndPose - userId: " + userId + ", pose: " + pose);
	}

}
